#Added Mutex
require "Benchmark"
require "thread"

#Added Math module
require "control_lib/math/mathmodule.rb"
include Mathmodule

#Added the Sensor modules
require "control_lib/level2/support/modules/dual_sensormodule.rb"
include Sensormodule

#need to set
#map.autoupdatemap(true)
#map.autologmap(true)
#at the main program


class Dualmap
  
  #start the mapping sequence
  def initialize(robotteam)
    
  #debug and testing  
  @debug =1
  @timescalled=0.0
  @avetime=0.0
  @total=0.0
    
  @updateinterval=0.5
  @loginterval=1.5
  @teamobj=robotteam
  @team=Array.new(robotteam.robotno)
    
  #map data   
  @mapwidth=600 
  @mapheight=600
  @cellwidth=0.08
  @cellheight=0.08  
  @initvalue=0.5
  @scanfre=180
  
  #robot data 
  @robotsize=0.5 
    
  #sensor infomation
  @lasermaxrange=19.0
 
  #used for the standard sensor model
  @laserrange=20.0
  @laserscale=10.0
  @laserdistsize=@laserrange*@laserscale
  @laserdist=Array.new()
    
  #Thread 
  @mapaccess= Mutex.new  
    
  #flags
  @update=0
  @log=0
  @logobject=1
  
  #autoscans
  @logobjectint=0.8
  
  #data transmission arrays
  @victim=[]
  @minvicdist=10.0
    
  #use ther true robot position
  @usetrue=true
  
   
  puts "Initializing map "  
  self.initmap()
       
  end
  
  def initmap()
   @maxwidth=@mapwidth/2
   @maxheight=@mapheight/2
   #make into a two dimension array 
   @Q1map=Array.new(@maxwidth) {Array.new(@maxheight)}
   @Q2map=Array.new(@maxwidth) {Array.new(@maxheight)}
   @Q3map=Array.new(@maxwidth) {Array.new(@maxheight)}
   @Q4map=Array.new(@maxwidth) {Array.new(@maxheight)}
   
   #initiate the values
   @Q1map.each{|member|
     member.map!{ @initvalue }
   }
   @Q2map.each{|member|
     member.map!{ @initvalue }
   }
   @Q3map.each{|member|
     member.map!{ @initvalue }
   }
   @Q4map.each{|member|
     member.map!{ @initvalue }
   }
   #@Q1map=x,y
  
   self.initsensormodel    
   #self.logobject # start the automatic log object
    
  end
 
 
  
  def getmap(x,y)
    return 0.5                 if(x>(@maxwidth) or y>(@maxheight) or x<-1*(@maxwidth) or y<-1*(@maxheight))
    return @Q1map[x][y].to_f   if(x>=0 and y>=0)
    return @Q2map[x][y].to_f   if(x>=0 and y<=0)
    return @Q3map[x][y].to_f   if(x<=0 and y<=0)
    return @Q4map[x][y].to_f   if(x<=0 and y>=0)
  end
  
  def putmap(x,y,value)
    if(x>(@maxwidth) or y>(@maxheight) or x<-1*(@maxwidth) or y<-1*(@maxheight))
      return false
    end
    if(x>=0 and y>=0)
       @Q1map[x][y]=value
       return true 
    end
     if(x>=0 and y<=0)
       @Q2map[x][y]=value
       return true 
    end
     if(x<=0 and y<=0)
       @Q3map[x][y]=value
       return true 
    end
     if(x<=0 and y>=0)
       @Q4map[x][y]=value
       return true 
    end
    
  end
  

  
  
  def autologmap(value)
     if (value==true)
      puts "starting auto update"
      @log=1
      self.autolog
      self.logobject
    else
      @log=0
    end
  end
  
  def autoupdatemap(value)
    if (value==true)
      puts "starting auto update"
      @update=1
      self.updatemap
    else
      @update=0
    end
  end
  
  
  def updatemap
    @m_update_t=Thread.new{
     Thread.current["name"]="map_update"
     while @update==1
       
     @team.each_with_index{|robot,index|  
     robot.recieve#update data on robot
     # @mapaccess.synchronize do     
     self.updatelaser(robot)
     # end
     }
     sleep(@updateinterval)  
     end
     
    }
  end
  
def updatelaser(robot)
    if(robot.moveflag=="forward")
          #self.ruby_maplaseroverwrite(robot)
          #self.ruby_maplaserincrement(robot)
          #self.ruby_maplaserbayes(robot)
          self.ruby_maplaserbayes_stand(robot,0.02)
    end
end

def standarddist(wallpos,distri)
 totalscan= @laserscale*@laserrange
 Mathmodule::math_standarddist(wallpos,totalscan,@laserscale,distri) 
end  
  
def initsensormodel()
  @laserdist=self.standarddist(10.0,0.1)
  puts "sensormodel initiated"
end
  
def bayesupdate(oldprob,sensorprob)
 return Mathmodule::math_bayesupdate(oldprob,sensorprob)
end  
  

def logobject
   @m_autoscan_t=Thread.new{
   Thread.current["name"]="map_autoscan"
    while @logobject==1  
      #log the objects
      @team.each_with_index{|robot,index|
      self.scanvictim(robot,index) #log the map changes to the data    
      self.mapins(robot,index)
      }
      self.logins()
      sleep(@logobjectint)  
    end
    }
  
end

  
def autolog
    @m_autolog_t=Thread.new{
   Thread.current["name"]="map_logmap"
    while @log==1  
      #@mapaccess.synchronize do
      self.logdata() #log the map changes to the data    
      #end
      sleep(@loginterval)  
    end
    }
  
end
  
  def scanvictim(robot,index)
      unless(robot.victsen=~/NoVictims/i)
      puts "Victim found"
      parts=(robot.victsen.size)/2.0
      puts "#{parts} parts found"
      
      #find the average position of the parts
      avex=0
      avey=0
      robot.victsen.each_with_index{|member,index|
        if(index%2==1) #location
          temparray=member.split(",")
          avex=avex+temparray[0].to_f
          avey=avey+temparray[1].to_f
        end
      }
      avex=avex/parts
      avey=avey/parts
      puts "Average position: x=#{avex} y=#{avey} "
      cellpos=self.converttocell(avex+robot.ins[0].to_f,avey+robot.ins[1].to_f)   
      #update map to make victim position  inaccessable
      2.times{|xoffset|
       2.times{|yoffset|
         self.putmap(cellpos[0]+(xoffset-1),cellpos[1]+(yoffset-1),0.90)
       }
      }
      #log the Victim position
      
      #if victim area is empty
      if(@victim.size==0)
           @victim<<cellpos
              puts "#{@victim.size} known"
      end
      
   #if not empty, remember only if it's size is more than @minvicdist meters from every member
    @victim.each_with_index { |member,index|
    if(dist(member[0],member[1],cellpos[0],cellpos[1])<@minvicdist) #not the same cell
      break
    end
    if(index+1==@victim.size) #able to insert
      @victim<<cellpos
          puts "#{@victim.size} victims known"
    end
    }

    end
  end

  def mapins(robot,num)
     cellpos=self.converttocell(robot.ins[0],robot.ins[1])
     self.putmap(cellpos[0],cellpos[1],0.0)
  end

 def converttocell(x,y)
     cellpos=[]
     cellpos<<(x.to_f/@cellwidth).to_i
     cellpos<<(y.to_f/@cellheight).to_i
     return cellpos
 end
   
  def logdata()
   logbench=Benchmark.measure{  
   #Prepare the data in an array first
   inputstring=""
     @mapwidth.times{|y|
         @mapheight.times{|x|
          inputstring<<(self.getmap(x-(@mapwidth/2),y-(@mapheight/2))).to_s<<","      
         }
         inputstring<<("\n")
    }     
      
   #log the map
    open("mapdata/occ_dual_data.dat","w")do|file|
     #put map size 
     file.puts("#{@mapwidth},#{@mapheight}")
     #put data into file
     file.print(inputstring)
   end
  
   
   }
   bench_array=logbench.to_a
   puts "logbench #{(bench_array[1].to_f+bench_array[2].to_f)}" if(@debug==1)
  end
  
 def logins()
       open("mapdata/occ_dual_obj.dat","w")do|file|
      #log the robotic INS
      @team.each_with_index{|robot,index|
      out2="" 
      out2<<robot.status[0]<<","<<robot.robotname<<",INS,"
      xycell=self.converttocell(robot.ins[0].to_f,robot.ins[1].to_f)
      out2<<xycell[0].to_s<<","<<xycell[1].to_s<<",0.0,"<<robot.ins[3]<<","<<robot.ins[4]<<","<<robot.ins[5]
      file.puts(out2)
      }
      #log the victims
      @victim.each_with_index { |member,index|
      out2=""
        if(@team[0])
          out2<<@team[0].status[0] #Base time on first robot
        else
          out2<<0.00
        end
        out2<<",VICTIM,"<<member[0].to_s<<","<<member[1].to_s
        file.puts(out2)
      }
   end
   
 end
 

end


#Test class
#testmap=Teammap.new()
#testmap.initmap
#testmap.putmap(1,0,0.3)
#testmap.putmap(-2,0,0.4)
#puts testmap.getmap(1,0)+testmap.getmap(-2,0)

